/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.bodies;

import ore.Frame;
import ore.Line;
import ore.Transform;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Vector3D;

/** Modeling of a one-axis ellipsoid.

 * <p>One-axis ellipsoids is a good approximate model for most planet-size
 * and larger natural bodies. It is the equilibrium shape reached by
 * a fluid body under its own gravity field when it rotates. The symmetry
 * axis is the rotation or polar axis.</p>

 * <p>This class is a simple adaptation of the <a
 * href="http://www.spaceroots.org/documents/distance/Ellipsoid.java">Ellipsoid</a>
 * example class implementing the algorithms described in the paper <a
 * href="http://www.spaceroots.org/documents/distance/distance-to-ellipse.pdf"> Quick
 * computation of the distance between a point and an ellipse</a>.</p>
 *
 * @author Luc Maisonobe
 * @version $Revision:1665 $ $Date:2008-06-11 12:12:59 +0200 (mer., 11 juin 2008) $
 */
public class OneAxisEllipsoid
    extends Object
    implements BodyShape
{

    private static final double ONE_THIRD = 1.0 / 3.0;

    /** 
     * Body frame related to body shape.
     */
    private final Frame bodyFrame;
    /** 
     * Equatorial radius.
     */
    private final double ae;
    /** 
     * Eccentricity power 2.
     */
    private final double e2;
    /** 
     * 1 minus flatness.
     */
    private final double g;
    /** 
     * g squared
     */
    private final double g2;
    /** 
     * Equatorial radius squared
     */
    private final double ae2;
    /**
     * Convergence limit.
     */
    private double closeApproachThreshold;
    /** 
     * Convergence limit.
     */
    private double angularThreshold;


    /** 
     * The following table provides conventional parameters for global
     * Earth models.
     * 
     * <table border="1" cellpadding="5">
     * <tr bgcolor="#ccccff"><font size="+3"><th>model</th><th>a<sub>e</sub> (m)</th><th>f</th></font></tr>
     * <tr><th bgcolor="#eeeeff">GRS 80</th><th>6378137.0</th><th>1.0 / 298.257222101</th></tr>
     * <tr><th bgcolor="#eeeeff">WGS84</th><th>6378137.0</th><th>1.0 / 298.257223563</th></tr>
     * </table>
     * 
     * @param ae Equatorial radius
     * @param f The flattening (f = (a-b)/a)
     * @param bodyFrame Body frame related to body shape
     * 
     * @see ore.frames.FramesFactory#getITRF2005()
     */
    public OneAxisEllipsoid(double ae, double f, Frame bodyFrame) {
        super();
        this.ae = ae;
        this.e2      = f * (2.0 - f);
        this.g       = 1.0 - f;
        this.g2      = g * g;
        this.ae2     = ae * ae;
        this.setCloseApproachThreshold(1.0e-10);
        this.setAngularThreshold(1.0e-14);
        this.bodyFrame = bodyFrame;
    }


    /**
     * The close approach threshold is a ratio used to identify
     * special cases in the {@link #transform(Vector3D, Frame,
     * AbsoluteDate)} method.
     * 
     * For d =
     * (x<sup>2</sup>+y<sup>2</sup>+z<sup>2</sup>)<sup>&frac12;</sup>,
     * the distance between the point and the ellipsoid center; and
     * a<sub>e</sub>, the equatorial radius of the ellipsoid:
     * 
     * <ul>
     * 
     *   <li> all points such that d&lt;&epsilon; a<sub>e</sub> are
     *        considered at the center</li>
     * 
     *   <li> all points closer to the surface of the ellipsoid than
     *        &epsilon; d are considered on the surface </li>
     * 
     * </ul>
     * 
     * If this method is not called, the default value is
     * 10<sup>-10</sup>.
     * 
     * @param closeApproachThreshold Close approach threshold (no unit)
     */
    public void setCloseApproachThreshold(double closeApproachThreshold) {
        this.closeApproachThreshold = closeApproachThreshold;
    }
    /** 
     * The angular threshold is the convergence threshold used to stop
     * the iterations in the {@link #transform(Vector3D, Frame,
     * AbsoluteDate)} method.
     * 
     * It applies directly to the latitude. When convergence is
     * reached, the real latitude is guaranteed to be between &phi; -
     * &delta;&phi/2 and &phi; + &delta;&phi/2 where &phi; is the
     * computed latitude and &delta;&phi is the angular threshold set
     * by this method.
     * 
     * If this method is not called, the default value is set to
     * 10<sup>-14</sup>.
     * 
     * @param angularThreshold Angular convergence threshold (rad)
     */
    public void setAngularThreshold(double angularThreshold) {
        this.angularThreshold = angularThreshold;
    }
    /** 
     * @return Equatorial radius of the body (m)
     */
    public double getEquatorialRadius() {
        return this.ae;
    }
    /** 
     * @return Body frame related to body shape
     */
    public Frame getBodyFrame() {
        return this.bodyFrame;
    }
    /**
     * {@inheritDoc}
     */
    public GeodeticPoint getIntersectionPoint(Line line, Vector3D close,
                                              Frame frame, AbsoluteDate date)
        throws OrekitException
    {
        /*
         * Transform line and close to body frame
         */
        Transform frameToBodyFrame = frame.getTransformTo(bodyFrame, date);
        Line lineInBodyFrame = frameToBodyFrame.transformLine(line);
        Vector3D closeInBodyFrame = frameToBodyFrame.transformPosition(close);
        double closeAbscissa = lineInBodyFrame.getAbscissa(closeInBodyFrame);
        /*
         * Compute some miscellaneous variables outside of the loop
         */
        Vector3D point    = lineInBodyFrame.getOrigin();
        double x          = point.getX();
        double y          = point.getY();
        double z          = point.getZ();
        double z2         = z * z;
        double r2         = x * x + y * y;

        Vector3D direction = lineInBodyFrame.getDirection();
        double dx         = direction.getX();
        double dy         = direction.getY();
        double dz         = direction.getZ();
        double cz2        = dx * dx + dy * dy;

        // abscissa of the intersection as a root of a 2nd degree polynomial :
        // a k^2 - 2 b k + c = 0
        double a  = 1.0 - e2 * cz2;
        double b  = -(g2 * (x * dx + y * dy) + z * dz);
        double c  = g2 * (r2 - ae2) + z2;
        double b2 = b * b;
        double ac = a * c;
        if (b2 < ac) {
            return null;
        }
        double s  = Math.sqrt(b2 - ac);
        double k1 = (b < 0) ? (b - s) / a : c / (b + s);
        double k2 = c / (a * k1);

        // select the right point
        double k =
            (Math.abs(k1 - closeAbscissa) < Math.abs(k2 - closeAbscissa)) ? k1 : k2;
        Vector3D intersection = lineInBodyFrame.pointAt(k);
        double ix = intersection.getX();
        double iy = intersection.getY();
        double iz = intersection.getZ();

        double lambda = Math.atan2(iy, ix);
        double phi    = Math.atan2(iz, g2 * Math.sqrt(ix * ix + iy * iy));
        return new GeodeticPoint(phi, lambda, 0.0);

    }

    /** Transform a surface-relative point to a cartesian point.
     * @param point surface-relative point
     * @return point at the same location but as a cartesian point
     */
    public Vector3D transform(GeodeticPoint point) {
        double longitude = point.getLongitude();
        double cLambda   = Math.cos(longitude);
        double sLambda   = Math.sin(longitude);
        double latitude  = point.getLatitude();
        double cPhi      = Math.cos(latitude);
        double sPhi      = Math.sin(latitude);
        double h         = point.getAltitude();
        double n         = ae / Math.sqrt(1.0 - e2 * sPhi * sPhi);
        double r         = (n + h) * cPhi;
        return new Vector3D(r * cLambda, r * sLambda, (g2 * n + h) * sPhi);
    }

    /**
     * Transform a cartesian point to a surface-relative point.
     * 
     * @param point cartesian point
     * @param frame frame in which cartesian point is expressed
     * @param date date of the point in given frame
     * 
     * @return Point at the same location but as a surface-relative
     * point, expressed in body frame
     * 
     * @exception OrekitException if point cannot be converted to body frame
     */
    public GeodeticPoint transform(Vector3D point, Frame frame,
                                   AbsoluteDate date)
        throws OrekitException
    {
        /*
         * Transform line to body frame
         */
        Vector3D pointInBodyFrame =
            frame.getTransformTo(bodyFrame, date).transformPosition(point);

        double z          = pointInBodyFrame.getZ();
        double z2         = z * z;
        double r2         = pointInBodyFrame.getX() * pointInBodyFrame.getX() +
                                  pointInBodyFrame.getY() * pointInBodyFrame.getY();
        double r          = Math.sqrt(r2);
        double g2r2ma2    = g2 * (r2 - ae2);
        double g2r2ma2pz2 = g2r2ma2 + z2;
        double dist       = Math.sqrt(r2 + z2);
        boolean inside    = g2r2ma2pz2 <= 0;

        /*
         * Point at the center
         */
        if (dist < (this.closeApproachThreshold * ae)) {
            return new GeodeticPoint(0.5 * Math.PI, 0.0, -ae * Math.sqrt(1.0 - e2));
        }
        else {
            double cz = r / dist;
            double sz = z / dist;
            double t = z / (dist + r);
            /*
             * Distance to the ellipse along the current line
             * as the smallest root of a 2nd degree polynomial
             *     a k^2 - 2 b k + c = 0
             */
            double a  = 1.0 - e2 * cz * cz;
            double b  = g2 * r * cz + z * sz;
            double c  = g2r2ma2pz2;
            double b2 = b * b;
            double ac = a * c;
            double k  = c / (b + Math.sqrt(b2 - ac));
            double lambda = Math.atan2(pointInBodyFrame.getY(), pointInBodyFrame.getX());
            double phi    = Math.atan2(z - k * sz, g2 * (r - k * cz));

            /*
             * Point on the ellipse
             */
            if (Math.abs(k) < (this.closeApproachThreshold * dist)) {
                return new GeodeticPoint(phi, lambda, k);
            }
            else {
                for (int iterations = 0; iterations < 100; ++iterations) {
                    /*
                     * 4th degree normalized polynom describing
                     * circle/ellipse intersections
                     *     tau^4 + b tau^3 + c tau^2 + d tau + e = 0
                     * (there is no need to compute e here)
                     */
                    a        = g2r2ma2pz2 + g2 * (2.0 * r + k) * k;
                    b        = -4.0 * k * z / a;
                    c        = 2.0 * (g2r2ma2pz2 + (1.0 + e2) * k * k) / a;
                    double d = b;
                    /*
                     * Reduce the polynomial to degree 3 by removing
                     * the already known real root t
                     *        tau^3 + b tau^2 + c tau + d = 0
                     */
                    b += t;
                    c += t * b;
                    d += t * c;
                    /*
                     * Find the other real root
                     */
                    b2       = b * b;
                    double Q = (3.0 * c - b2) / 9.0;
                    double R = (b * (9.0 * c - 2.0 * b2) - 27.0 * d) / 54.0;
                    double D = Q * Q * Q + R * R;
                    double tildeT;
                    double tildePhi;
                    if (D >= 0) {
                        double rootD = Math.sqrt(D);
                        double rMr = R - rootD;
                        double rPr = R + rootD;

                        tildeT = Math.cbrt(rPr) + Math.cbrt(rMr) - b * ONE_THIRD;

                        double tildeT2   = tildeT * tildeT;
                        double tildeT2P1 = 1.0 + tildeT2;
                        tildePhi = Math.atan2(z * tildeT2P1 - 2 * k * tildeT,
                                              g2 * (r * tildeT2P1 - k * (1.0 - tildeT2)));
                    }
                    else {
                        Q = -Q;
                        double qRoot     = Math.sqrt(Q);
                        double theta     = Math.acos(R / (Q * qRoot));

                        /*
                         * first root based on (theta / 3),
                         */
                        tildeT           = 2.0 * qRoot * Math.cos(theta * ONE_THIRD) - b * ONE_THIRD;
                        double tildeT2   = tildeT * tildeT;
                        double tildeT2P1 = 1.0 + tildeT2;
                        tildePhi         = Math.atan2(z * tildeT2P1 - 2 * k * tildeT,
                                                      g2 * (r * tildeT2P1 - k * (1.0 - tildeT2)));
                        if ((tildePhi * phi) < 0) {
                            /*
                             * the first root was on the wrong hemisphere,
                             * try the second root based on ((theta + 2PI) / 3)
                             */
                            tildeT    = 2.0 * qRoot * Math.cos((theta + 2.0 * Math.PI) * ONE_THIRD) - b * ONE_THIRD;
                            tildeT2   = tildeT * tildeT;
                            tildeT2P1 = 1.0 + tildeT2;
                            tildePhi  = Math.atan2(z * tildeT2P1 - 2 * k * tildeT,
                                                   g2 * (r * tildeT2P1 - k * (1.0 - tildeT2)));
                            if (tildePhi * phi < 0) {
                                /*
                                 * The second root was on the wrong  hemisphere,
                                 * try the third (and last) root based on ((theta + 4PI) / 3)
                                 */
                                tildeT    = 2.0 * qRoot * Math.cos((theta + 4.0 * Math.PI) * ONE_THIRD) - b * ONE_THIRD;
                                tildeT2   = tildeT * tildeT;
                                tildeT2P1 = 1.0 + tildeT2;
                                tildePhi  = Math.atan2(z * tildeT2P1 - 2 * k * tildeT,
                                                       g2 * (r * tildeT2P1 - k * (1.0 - tildeT2)));
                            }
                        }
                    }

                    /*
                     * Midpoint on the ellipse
                     */
                    double dPhi  = Math.abs(0.5 * (tildePhi - phi));
                    phi          = 0.5 * (phi + tildePhi);
                    double cPhi  = Math.cos(phi);
                    double sPhi  = Math.sin(phi);
                    double coeff = Math.sqrt(1.0 - e2 * sPhi * sPhi);

                    if (dPhi < this.angularThreshold) {
                        /*
                         * Angular convergence reached
                         */
                        return new GeodeticPoint(phi, lambda,
                                                 r * cPhi + z * sPhi - ae * coeff);
                    }
                    else {
                        b = ae / coeff;
                        double dR = r - cPhi * b;
                        double dZ = z - sPhi * b * g2;
                        k = Math.sqrt(dR * dR + dZ * dZ);
                        if (inside) {
                            k = -k;
                        }
                        t = dZ / (k + dR);
                    }
                }
                throw new RuntimeException("unable to converge");
            }
        }
    }

}
